#!/usr/bin/env python
# -*- coding: utf-8 -*-

import socket
import rospy
from std_msgs.msg import String

# 定义服务器地址和端口号
server_address = ('0.0.0.0', 9999)

# 创建一个TCP服务器
server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server_socket.bind(server_address)
server_socket.listen(1)

# 接受客户端连接
client_connection, client_address = server_socket.accept()

# ROS节点初始化
rospy.init_node('cmd_receive_node', anonymous=True)

# 创建发布者
command_pub = rospy.Publisher('receive_cmd', String, queue_size=10)

def receive_commands():
    try:
        while not rospy.is_shutdown():
            # 接收数据
            data = client_connection.recv(1024).decode()
            if not data:
                continue
            
            rospy.loginfo(data)
            
            # 发布命令到ROS话题
            command_pub.publish(data)
            
    finally:
        # 关闭连接
        client_connection.close()
        server_socket.close()

if __name__ == '__main__':
    try:
        receive_commands()
    except rospy.ROSInterruptException:
        pass